//////////////////////////////////////////////////////////////////////////////////
//                                                                              //
//  This file is part of the buola project (https://code.google.com/p/buola/).  //
//                                                                              //
//  Copyright(c) 2007-2012 Xavi Gratal                                          //
//  gratal AT gmail DOT com                                                     //
//                                                                              //
//  Buola is free software: you can redistribute it and/or modify               //
//  it under the terms of the GNU General Public License as published by        //
//  the Free Software Foundation, either version 3 of the License, or           //
//  (at your option) any later version.                                         //
//                                                                              //
//  Buola is distributed in the hope that it will be useful,                    //
//  but WITHOUT ANY WARRANTY; without even the implied warranty of              //
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the               //
//  GNU General Public License for more details.                                //
//                                                                              //
//  You should have received a copy of the GNU General Public License           //
//  along with buola.  If not, see <http://www.gnu.org/licenses/>.              //
//                                                                              //
//////////////////////////////////////////////////////////////////////////////////

#include "cnode.h"
#include <buola/scene/cmeshbuilder.h>
#include <buola/scene/transform/crttransform.h>
#include <buola/scene/transform/crevolutionjoint.h>
#include <buola/scene/cgeode.h>
#include <buola/scene/geodes/ccylinder.h>
#include <buola/scene/geodes/ccapsule.h>
#include <buola/scene/cmaterial.h>
#include <buola/scene/materials/csolidmaterial.h>

namespace buola { namespace scene { namespace vrml {

void CNode::Print(int pDepth)
{
    if(mType.mToken==TOKEN_INVALID) return;
    for(int i=0;i<pDepth;i++)
        msg_info() << "   ";
    msg_info() << mType.Text() << "\n";
    for(const PField &f : mFields)
    {
        if(f->mNodes.size())
        {
            for(const PNode &n : f->mNodes)
            {
                n->Print(pDepth+1);
            }
        }
    }
}

PField CNode::GetField(const std::string& pName)
{
    for(const PField &f : mFields)
    {
        if(f->mIdentifier.Text()==pName) return f;
    }
    
    return nullptr;
}

void CNode::ProcessDefUseIs(std::map<std::string,PNode> &pDefMap,std::map<std::string,PField> &pIsMap)
{
    if(mType.mToken==TOKEN_INVALID) throw XInvalid("error in ProcessDefUse");

    if(mDef.mToken!=TOKEN_INVALID)
        pDefMap[mDef.Text()]=this;
    
    for(PField &f : mFields)
    {
        if(f->mIs.mToken!=TOKEN_INVALID)
        {
            auto lFind=pIsMap.find(f->mIs.Text());
            if(lFind==pIsMap.end()) throw XNotFound("IS for undefined field");
            f->mNodes=lFind->second->mNodes;
            f->mTokens=lFind->second->mTokens;
        }
        else if(f->mNodes.size())
        {
            for(PNode &n : f->mNodes)
            {
                if(n->mUse.mToken!=TOKEN_INVALID)
                {
                    auto lFind=pDefMap.find(n->mUse.Text());
                    if(lFind==pDefMap.end()) throw XNotFound("USE for undefined node");
                    n=lFind->second;
                }
                else
                {
                    n->ProcessDefUseIs(pDefMap,pIsMap);
                }
            }
        }
    }
}

bool CNode::IsServo()
{
    std::string lType=mType.Text();
    
    return lType=="Servo";
}

bool CNode::IsTransform()
{
    std::string lType=mType.Text();
    
    return lType=="Robot"||lType=="Transform"||lType=="LED"||lType=="GPS"||lType=="Emitter"||
           lType=="Receiver"||lType=="Accelerometer"||lType=="Gyro"||lType=="InertialUnit"||lType=="DistanceSensor"||
           lType=="TouchSensor"||lType=="Camera";
}

bool CNode::IsShape()
{
    std::string lType=mType.Text();
    
    return lType=="Shape";
}

PRTTransform CNode::ExtractTransform()
{
    if(!IsTransform()) throw XInvalid("wrong node in ExtractTransform");

    PRTTransform lTransform=new CRTTransform;
    
    for(const PField &f : mFields)
    {
        std::string lName=f->mIdentifier.Text();
        
        if(lName=="children")
        {
            for(const PNode &n : f->mNodes)
            {
                if(n->IsServo())
                {
                    lTransform->AddChild(n->ExtractServo());
                }
                else if(n->IsTransform())
                {
                    lTransform->AddChild(n->ExtractTransform());
                }
                else if(n->IsShape())
                {
                    PObject lShape=n->ExtractShape();
                    if(lShape)
                        lShape->AttachTo(lTransform);
                }
                else
                {            
                    msg_info() << "unknown child type " << n->mType.Text() << "\n";
                }
            }
        }
        else if(lName=="name")
        {
        }
        else if(lName=="translation")
        {
            lTransform->SetTranslation(f->ExtractVector());
        }
        else if(lName=="rotation")
        {
            lTransform->SetRotation(f->ExtractRotation());
        }
        else
        {
            //msg_info() << "unknown transform field type " << lName << "\n";
        }
    }
    
    return lTransform;
}

PRevolutionJoint CNode::ExtractServo()
{
    if(!IsServo()) throw XInvalid("wrong node in ExtractTransform");

    PRevolutionJoint lServo=new CRevolutionJoint({0,0,0},{0,1,0});
    
    std::string lName;
    
    for(const PField &f : mFields)
    {
        std::string lName=f->mIdentifier.Text();
        
        if(lName=="children")
        {
            for(const PNode &n : f->mNodes)
            {
                if(n->IsServo())
                {
                    lServo->AddChild(n->ExtractServo());
                }
                else if(n->IsTransform())
                {
                    lServo->AddChild(n->ExtractTransform());
                }
                else if(n->IsShape())
                {
                    PObject lShape=n->ExtractShape();
                    if(lShape)
                        lShape->AttachTo(lServo);
                }
                else
                {            
                    msg_info() << "unknown child type " << n->mType.Text() << "\n";
                }
            }
        }
        else if(lName=="name")
        {
            lServo->SetName(f->ExtractString());
        }
        else if(lName=="translation")
        {
            lServo->SetTranslation(f->ExtractVector());
        }
        else if(lName=="rotation")
        {
            lServo->SetAxis(f->ExtractAxis());
            lServo->SetOffset(f->ExtractOffset());
        }
        else
        {
            //msg_info() << "unknown transform field type " << lName << "\n";
        }
    }
    
    return lServo;
}

PObject CNode::ExtractShape()
{
    if(!IsShape()) throw XInvalid("wrong node in ExtractShape");

    PGeode lObject=nullptr;
    PMaterial lMaterial=nullptr;
    
    for(const PField &f : mFields)
    {
        std::string lName=f->mIdentifier.Text();
        
        if(lName=="geometry")
        {
            if(f->mNodes.size()!=1) throw XInvalid("wrong geometry field");
            
            PNode lGeometryNode=f->mNodes[0];
            std::string lType=lGeometryNode->mType.Text();
            
            if(lType=="IndexedFaceSet")
            {
                lObject=lGeometryNode->ExtractGeode();
            }
            else if(lType=="Cylinder")
            {
                lObject=lGeometryNode->ExtractCylinder();
            }
            else if(lType=="Capsule")
            {
                lObject=lGeometryNode->ExtractCapsule();
            }
            else
            {
                msg_info() << "unknown geometry type " << lType << "\n";
            }
        }
        else if(lName=="appearance")
        {
            if(f->mNodes.size()!=1) throw XInvalid("wrong appearance field");
            
            lMaterial=f->mNodes[0]->ExtractMaterial();
        }
        else
        {
            msg_info() << "unknown shape field type " << lName << "\n";
        }
    }
    
    if(lObject&&lMaterial) lObject->SetMaterial(lMaterial);

    return lObject;
}

PGeode CNode::ExtractGeode()
{
    CMeshBuilder lBuilder;
    
    std::vector<mat::CVec3d> lPoints;
    std::vector<int> lIndices;
    std::vector<int> lFaces;

    bool lReverse=false;
    
    for(const PField &f : mFields)
    {
        if(f->mIdentifier.Text()=="ccw")
        {
            if(f->mTokens[0].mToken==TOKEN_KW_FALSE)
                lReverse=true;
        }
        else if(f->mIdentifier.Text()=="coord")
        {
            if(f->mNodes.size()!=1)
                throw XInvalid("wrong coord field 1");
            lPoints=f->mNodes[0]->ExtractCoordinates();
        }
        else if(f->mIdentifier.Text()=="coordIndex")
        {
            lFaces.push_back(0);
            for(const CToken &t : f->mTokens)
            {
                int lIndex=to_<int>(t.Text());
                if(lIndex==-1)
                {
                    if(lReverse)
                        std::reverse(lIndices.begin()+lFaces.back(),lIndices.end());
                    lFaces.push_back(lIndices.size());
                }
                else
                    lIndices.push_back(lIndex);
            }
        }
    }
    
    lBuilder.SetPosition(lPoints);
    
    std::vector<int> lSMFaces;
    for(int i=0;i<lFaces.size()-1;i++)
        lSMFaces.push_back(i);

    lBuilder.SetVertices(lIndices,lIndices);
    lBuilder.SetFaces(lFaces);
    lBuilder.CalcNormals();
    lBuilder.SmoothNormals();
    lBuilder.AddSubMesh("name",lSMFaces,EPrimitive::TRIANGLE_FAN);
    
    return new CGeode(lBuilder.BuildMesh());
}

PGeode CNode::ExtractCylinder()
{
    PField lField=GetField("radius");
    if(!lField) throw XInvalid("wrong cylinder node");
    double lRadius=lField->ExtractNumber();
    lField=GetField("height");
    if(!lField) throw XInvalid("wrong cylinder field");
    double lHeight=lField->ExtractNumber();
    return new CCylinder(lHeight,lRadius);
}

PGeode CNode::ExtractCapsule()
{
    PField lField=GetField("radius");
    if(!lField) throw XInvalid("wrong cylinder node");
    double lRadius=lField->ExtractNumber();
    lField=GetField("height");
    if(!lField) throw XInvalid("wrong cylinder field");
    double lHeight=lField->ExtractNumber();
    return new CCapsule(lHeight,lRadius);
}

PMaterial CNode::ExtractMaterial()
{
    img::CColor lDiffuseColor;
    img::CColor lSpecularColor;
    
    for(const PField &f : mFields)
    {
        if(f->mIdentifier.Text()=="material")
        {
            if(f->mNodes.size()!=1) throw XInvalid("wrong material node");
            
            PNode lMaterialNode=f->mNodes[0];
            
            for(const PField &g : lMaterialNode->mFields)
            {
                if(g->mIdentifier.Text()=="diffuseColor")
                {
                    lDiffuseColor=g->ExtractColor();
                }
                else if(g->mIdentifier.Text()=="specularColor")
                {
                    lSpecularColor=g->ExtractColor();
                }
                else if(g->mIdentifier.Text()=="shininess")
                {
                    
                }
                else
                {
                    msg_info() << g->mIdentifier.Text() << "\n";
                }
            }
        }
        else if(f->mIdentifier.Text()=="texture")
        {
            
        }
        else
        {
            msg_info() << f->mIdentifier.Text() << "\n";
        }
    }

    if(lDiffuseColor.Valid())
    {
        return new CSolidMaterial(lDiffuseColor);
    }
    else
    {
        return nullptr;
    }
}

std::vector<mat::CVec3d> CNode::ExtractCoordinates()
{
    if(mType.Text()!="Coordinate"||mFields.size()!=1)
        throw XInvalid("wrong coord field 2");
    
    PField lField=mFields[0];
    
    if(lField->mIdentifier.Text()!="point")
        throw XInvalid("wrong coord field 3");
    
    if(lField->mTokens.size()%3)
        throw XInvalid("wrong coord field 4");
    
    std::vector<buola::mat::CVec3d> lCoordinates;
    
    for(int i=0;i<lField->mTokens.size();i+=3)
    {
        lCoordinates.emplace_back(to_<double>(lField->mTokens[i].Text()),
                                  to_<double>(lField->mTokens[i+1].Text()),
                                  to_<double>(lField->mTokens[i+2].Text()));
    }
    
    return lCoordinates;
}
    
/*namespace vrml*/ } /*namespace scene*/ } /*namespace buola*/ }
